import lejos.nxt.Button;
import lejos.nxt.ColorLightSensor;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.Sound;
import lejos.nxt.comm.RConsole;

class Main {

    private static int NUM_RECORDS = 3 * 1024;
    private static int NUM_FIELDS = 4;
    private static int RESULT_SIZE = NUM_RECORDS * NUM_FIELDS;
    private static int RESULT_SIZE_M1 = RESULT_SIZE - 1;
    private static int[] RESULTS = new int[RESULT_SIZE];

    private static int counter;
    private static long time;
    private static long BALANCE_DURATION = 10000;
    private static long PRE_BALANCE_DURATION = 1000;
    private static int FALL = 75;

    private static String DATA_HEADERS = ",count,error,pOut,iOut,dOut";
    private static String CONFIG_HEADERS = "KP,KI,KD,SCALE,TIME";
    private static String COMMA = ",";
    private static String NL = "\n";

    private static int idealValue;
    private static int error;
    private static int previousError;
    private static int integralError;

    private static int power;
    private static Motor leftMotor = Motor.C;
    private static Motor rightMotor = Motor.B;

    private static ColorLightSensor leftLightDetector = new ColorLightSensor(
	    SensorPort.S2, ColorLightSensor.TYPE_LIGHT_ACTIVE);
    private static ColorLightSensor rightLightDetector = new ColorLightSensor(
	    SensorPort.S3, ColorLightSensor.TYPE_LIGHT_ACTIVE);

    private static int KP = 30;
    private static int KI = 4;// 5;
    private static int KD = 35;// 35;
    private static int SCALE = 10;

    private static int pOut, iOut, dOut;

    public static void main(String[] args) {

	leftMotor.regulateSpeed(false);
	rightMotor.regulateSpeed(false);
	leftMotor.shutdown();
	rightMotor.shutdown();
	leftMotor.setPower(0);
	rightMotor.setPower(0);
	leftMotor.forward();
	rightMotor.forward();

	leftLightDetector.setFloodlight(true);
	rightLightDetector.setFloodlight(true);

	// otherStuff();

	System.out.print("Balance and press ENTER=");
	Button.ENTER.waitForPressAndRelease();
	// try {
	// Thread.sleep(PRE_BALANCE_DURATION);
	// } catch (InterruptedException e) {
	// System.err.println(e.getMessage());
	// }
	// Sound.beep();
	// time = System.currentTimeMillis();
	// for (counter = 0; (System.currentTimeMillis() - time) <
	// BALANCE_DURATION; ++counter) {
	// idealValue += leftLightDetector.readRawValue()
	// + rightLightDetector.readRawValue();
	// }
	// Sound.beepSequenceUp();
	// idealValue /= counter;
	idealValue = 590;
	System.out.println(idealValue);

	System.out.println("Press ENTER to run");
	Button.ENTER.waitForPressAndRelease();

	time = System.currentTimeMillis();
	for (counter = -1; counter < RESULT_SIZE_M1
		&& !Button.ESCAPE.isPressed();) {

	    error = leftLightDetector.readRawValue()
		    + rightLightDetector.readRawValue() - idealValue;
	    if (Math.abs(error) > FALL) {
		Sound.beepSequence();
		break;
	    }

	    RESULTS[++counter] = error;

	    // if (error < 0)
	    // error = error * 16 / 10;

	    integralError += error;
	    integralError = integralError * 2 / 3;

	    power = ((pOut = KP * error) + (iOut = KI * integralError) + (dOut = KD
		    * (error - previousError)))
		    / SCALE;
	    RESULTS[++counter] = pOut;
	    RESULTS[++counter] = iOut;
	    RESULTS[++counter] = dOut;

	    /*
	     * if (power > FALL_POWER || power < -FALL_POWER) {
	     * System.out.println("error=" + error + "\npower=" + power); break;
	     * } else if (power > 100) power = 100; else if (power < -100) power
	     * = -100;
	     */

	    leftMotor.setPower(power);
	    rightMotor.setPower(power);

	    previousError = error;
	    // if (counter == RESULT_SIZE_M1)
	    // counter = -1;
	}
	time = System.currentTimeMillis() - time;
	leftMotor.setPower(0);
	rightMotor.setPower(0);
	leftLightDetector.setFloodlight(false);
	rightLightDetector.setFloodlight(false);

	RConsole.openBluetooth(0);

	RConsole.println(CONFIG_HEADERS);
	RConsole.println(KP + COMMA + KI + COMMA + KD + COMMA + SCALE + COMMA
		+ time / 1000. + NL);
	RConsole.println(DATA_HEADERS);
	for (int i = 0; i < counter / NUM_FIELDS + 1; ++i) {
	    RConsole.print(COMMA + i);
	    for (int j = 0; j < NUM_FIELDS; ++j) {
		RConsole.print(COMMA + RESULTS[i * NUM_FIELDS + j]);
	    }
	    RConsole.print(NL);
	}

	RConsole.close();

	// Button.waitForPress(10000);
    }

    private static void otherStuff() {
	RConsole.openBluetooth(0);

	while (!Button.ESCAPE.isPressed()) {
	    RConsole.println(leftLightDetector.getLightValue() + " "
		    + leftLightDetector.getNormalizedLightValue() + " "
		    + leftLightDetector.readValue() + " "
		    + leftLightDetector.readRawValue());
	    RConsole.println(rightLightDetector.getLightValue() + " "
		    + rightLightDetector.getNormalizedLightValue() + " "
		    + rightLightDetector.readValue() + " "
		    + rightLightDetector.readRawValue());
	    RConsole.print(NL);

	}

	RConsole.close();
	System.exit(-1);
    }
}
